home *** CD-ROM | disk | FTP | other *** search
/ io Programmo 60 / IOPROG_60.ISO / soft / c++ / gsl-1.1.1-setup.exe / {app} / src / poly / qr.c < prev    next >
Encoding:
C/C++ Source or Header  |  2002-04-18  |  5.2 KB  |  246 lines
  1. /* poly/qr.c
  2.  * 
  3.  * Copyright (C) 1996, 1997, 1998, 1999, 2000 Brian Gough
  4.  * 
  5.  * This program is free software; you can redistribute it and/or modify
  6.  * it under the terms of the GNU General Public License as published by
  7.  * the Free Software Foundation; either version 2 of the License, or (at
  8.  * your option) any later version.
  9.  * 
  10.  * This program is distributed in the hope that it will be useful, but
  11.  * WITHOUT ANY WARRANTY; without even the implied warranty of
  12.  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13.  * General Public License for more details.
  14.  * 
  15.  * You should have received a copy of the GNU General Public License
  16.  * along with this program; if not, write to the Free Software
  17.  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  18.  */
  19.  
  20. static int qr_companion (double *h, size_t nc, gsl_complex_packed_ptr z);
  21.  
  22. static int
  23. qr_companion (double *h, size_t nc, gsl_complex_packed_ptr zroot)
  24. {
  25.   double t = 0.0;
  26.  
  27.   size_t iterations, e, i, j, k, m;
  28.  
  29.   double w, x, y, s, z;
  30.  
  31.   double p = 0, q = 0, r = 0; 
  32.  
  33.   /* FIXME: if p,q,r, are not set to zero then the compiler complains
  34.      that they ``might be used uninitialized in this
  35.      function''. Looking at the code this does seem possible, so this
  36.      should be checked. */
  37.  
  38.   int notlast;
  39.  
  40.   size_t n = nc;
  41.  
  42. next_root:
  43.  
  44.   if (n == 0)
  45.     return GSL_SUCCESS ;
  46.  
  47.   iterations = 0;
  48.  
  49. next_iteration:
  50.  
  51.   for (e = n; e >= 2; e--)
  52.     {
  53.       double a1 = fabs (FMAT (h, e, e - 1, nc));
  54.       double a2 = fabs (FMAT (h, e - 1, e - 1, nc));
  55.       double a3 = fabs (FMAT (h, e, e, nc));
  56.  
  57.       if (a1 <= GSL_DBL_EPSILON * (a2 + a3))
  58.     break;
  59.     }
  60.  
  61.   x = FMAT (h, n, n, nc);
  62.  
  63.   if (e == n)
  64.     {
  65.       GSL_SET_COMPLEX_PACKED (zroot, n-1, x + t, 0); /* one real root */
  66.       n--;
  67.       goto next_root;
  68.       /*continue;*/
  69.     }
  70.  
  71.   y = FMAT (h, n - 1, n - 1, nc);
  72.   w = FMAT (h, n - 1, n, nc) * FMAT (h, n, n - 1, nc);
  73.  
  74.   if (e == n - 1)
  75.     {
  76.       p = (y - x) / 2;
  77.       q = p * p + w;
  78.       y = sqrt (fabs (q));
  79.  
  80.       x += t;
  81.  
  82.       if (q > 0)        /* two real roots */
  83.     {
  84.       if (p < 0)
  85.         y = -y;
  86.       y += p;
  87.  
  88.       GSL_SET_COMPLEX_PACKED (zroot, n-1, x - w / y, 0);
  89.           GSL_SET_COMPLEX_PACKED (zroot, n-2, x + y, 0);
  90.     }
  91.       else
  92.     {
  93.       GSL_SET_COMPLEX_PACKED (zroot, n-1, x + p, -y);
  94.       GSL_SET_COMPLEX_PACKED (zroot, n-2, x + p, y);
  95.     }
  96.       n -= 2;
  97.  
  98.       goto next_root;
  99.       /*continue;*/
  100.     }
  101.  
  102.   /* No more roots found yet, do another iteration */
  103.  
  104.   if (iterations == 60)  /* increased from 30 to 60 */
  105.     {
  106.       /* too many iterations - give up! */
  107.  
  108.       return GSL_FAILURE ;
  109.     }
  110.  
  111.   if (iterations % 10 == 0 && iterations > 0)
  112.     {
  113.       /* use an exceptional shift */
  114.  
  115.       t += x;
  116.  
  117.       for (i = 1; i <= n; i++)
  118.     {
  119.       FMAT (h, i, i, nc) -= x;
  120.     }
  121.  
  122.       s = fabs (FMAT (h, n, n - 1, nc)) + fabs (FMAT (h, n - 1, n - 2, nc));
  123.       y = 0.75 * s;
  124.       x = y;
  125.       w = -0.4375 * s * s;
  126.     }
  127.  
  128.   iterations++;
  129.  
  130.   for (m = n - 2; m >= e; m--)
  131.     {
  132.       double a1, a2, a3;
  133.  
  134.       z = FMAT (h, m, m, nc);
  135.       r = x - z;
  136.       s = y - z;
  137.       p = FMAT (h, m, m + 1, nc) + (r * s - w) / FMAT (h, m + 1, m, nc);
  138.       q = FMAT (h, m + 1, m + 1, nc) - z - r - s;
  139.       r = FMAT (h, m + 2, m + 1, nc);
  140.       s = fabs (p) + fabs (q) + fabs (r);
  141.       p /= s;
  142.       q /= s;
  143.       r /= s;
  144.  
  145.       if (m == e)
  146.     break;
  147.       
  148.       a1 = fabs (FMAT (h, m, m - 1, nc));
  149.       a2 = fabs (FMAT (h, m - 1, m - 1, nc));
  150.       a3 = fabs (FMAT (h, m + 1, m + 1, nc));
  151.  
  152.       if (a1 * (fabs (q) + fabs (r)) <= GSL_DBL_EPSILON * fabs (p) * (a2 + a3))
  153.         break;
  154.     }
  155.  
  156.   for (i = m + 2; i <= n; i++)
  157.     {
  158.       FMAT (h, i, i - 2, nc) = 0;
  159.     }
  160.  
  161.   for (i = m + 3; i <= n; i++)
  162.     {
  163.       FMAT (h, i, i - 3, nc) = 0;
  164.     }
  165.  
  166.   /* double QR step */
  167.  
  168.   for (k = m; k <= n - 1; k++)
  169.     {
  170.       notlast = (k != n - 1);
  171.  
  172.       if (k != m)
  173.     {
  174.       p = FMAT (h, k, k - 1, nc);
  175.       q = FMAT (h, k + 1, k - 1, nc);
  176.       r = notlast ? FMAT (h, k + 2, k - 1, nc) : 0.0;
  177.  
  178.       x = fabs (p) + fabs (q) + fabs (r);
  179.  
  180.       if (x == 0)
  181.         continue;        /* FIXME????? */
  182.  
  183.       p /= x;
  184.       q /= x;
  185.       r /= x;
  186.     }
  187.  
  188.       s = sqrt (p * p + q * q + r * r);
  189.  
  190.       if (p < 0)
  191.     s = -s;
  192.  
  193.       if (k != m)
  194.     {
  195.       FMAT (h, k, k - 1, nc) = -s * x;
  196.     }
  197.       else if (e != m)
  198.     {
  199.       FMAT (h, k, k - 1, nc) *= -1;
  200.     }
  201.  
  202.       p += s;
  203.       x = p / s;
  204.       y = q / s;
  205.       z = r / s;
  206.       q /= p;
  207.       r /= p;
  208.  
  209.       /* do row modifications */
  210.  
  211.       for (j = k; j <= n; j++)
  212.     {
  213.       p = FMAT (h, k, j, nc) + q * FMAT (h, k + 1, j, nc);
  214.  
  215.       if (notlast)
  216.         {
  217.           p += r * FMAT (h, k + 2, j, nc);
  218.           FMAT (h, k + 2, j, nc) -= p * z;
  219.         }
  220.  
  221.       FMAT (h, k + 1, j, nc) -= p * y;
  222.       FMAT (h, k, j, nc) -= p * x;
  223.     }
  224.  
  225.       j = (k + 3 < n) ? (k + 3) : n;
  226.  
  227.       /* do column modifications */
  228.  
  229.       for (i = e; i <= j; i++)
  230.     {
  231.       p = x * FMAT (h, i, k, nc) + y * FMAT (h, i, k + 1, nc);
  232.  
  233.       if (notlast)
  234.         {
  235.           p += z * FMAT (h, i, k + 2, nc);
  236.           FMAT (h, i, k + 2, nc) -= p * r;
  237.         }
  238.       FMAT (h, i, k + 1, nc) -= p * q;
  239.       FMAT (h, i, k, nc) -= p;
  240.     }
  241.     }
  242.  
  243.   goto next_iteration;
  244. }
  245.  
  246.